package com.sholmes.android.scribbler;

import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.util.UUID;

import android.app.Service;
import android.bluetooth.BluetoothAdapter;
import android.bluetooth.BluetoothDevice;
import android.bluetooth.BluetoothSocket;
import android.content.Intent;
import android.os.IBinder;
import android.os.RemoteCallbackList;
import android.os.RemoteException;
import android.util.Log;

/**
 * Handles the bluetoothSocket connection to the robot.
 *  
 * @author Justin A. McCoy {@link mailto:justin.mccoy@acm.org}
 *
 */
@SuppressWarnings("unused")
public class ScribblerService extends Service{
	private static final String TAG = "------>ScribblerService: ";
	
	/**
	? ? ?* This is a list of callbacks that have been registered with the
	? ? ?* service. ?Note that this is package scoped (instead of private) so
	? ? ?* that it can be accessed more efficiently from inner classes.
	? ? ?*/	
	final RemoteCallbackList<ITestService> mCallbacks
		= new RemoteCallbackList<ITestService>();

	
	// Constants that indicate the current connection state
    public static final int STATE_DISCONNECTED = 0;       // we're doing nothing    
    public static final int STATE_CONNECTING = 2; // now initiating an outgoing connection
    public static final int STATE_CONNECTED = 3;  // now connected to a remote device

	//UUID required to communicate over the Bluetooth Serial Protocol
    private static final UUID SPP_UUID = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
    
    
    private int REVERSE_SPEED = 50;			//0 is the fastest speed and 100 is at full stop
    
    // Member fields
    private BluetoothAdapter _mAdapter;
    
    private ConnectThread mConnectThread;
    private ConnectedThread mConnectedThread;
    private byte[] sensors;
    private int _mState;
    
    //Use to handle all robot methods drive/turn left / get sensors.
    private Robot robot;
    
	private final ITestService.Stub mBinder = new ITestService.Stub() {
		
		@Override
		public int getState() throws RemoteException {
			Log.d(TAG, "getState");
			return 0;
		}
		
		@Override
		public void connect(String deviceAddress) throws RemoteException {
			Log.d(TAG,"SHOULD NOT SEE THIS MESSAGE___Connecting to device: " + deviceAddress);
			
		}

		@Override
		public void disconnect() throws RemoteException {
			Log.d(TAG,"SHOULD NOT SEE THIS MESSAGE___Disconnecting from Device: ");
			
		}	
		@Override
		public void unregisterCallback(ITestService cb)
				throws RemoteException {
			if (cb != null){
				mCallbacks.register(cb);
			}
			Log.d(TAG,"Unregistering listener");
		}
		
		@Override
		public void registerCallback(ITestService cb)
				throws RemoteException {
			if (cb != null){
				mCallbacks.unregister(cb);
			}
			Log.d(TAG,"Registering listener");
		}

		@Override
		public void connected(String device) throws RemoteException {			
		}

		@Override
		public void connecting(String device) throws RemoteException {			
		}

		@Override
		public void disconnected(String message) throws RemoteException {			
		}

		@Override
		public void drive(int speed, int direction) throws RemoteException {
		}

		@Override
		public void stop() throws RemoteException {
		}

		@Override
		public int[] getSensors() throws RemoteException {
			return null;
		}

		
	};
		
	@Override
	public IBinder onBind(Intent intent) {			
		// Select the interface to return. ?If your service only implements
		// a single interface, you can just return it here without checking
		// the Intent.


		return new ITestService.Stub() {
			
			@Override
			public void unregisterCallback(ITestService cb)
					throws RemoteException {
				if (cb != null){
					mCallbacks.unregister(cb);
				}
				Log.d(TAG,"Unregistering Scribbler service listener");
			}
			
			@Override
			public void registerCallback(ITestService cb)
					throws RemoteException {
				if (cb != null){
					mCallbacks.register(cb);					
				}
				Log.d(TAG,"Registering Scribbler service listener");
			}
			
			@Override
			public int getState() throws RemoteException {
				return 0;
			}
			
			@Override
			public void disconnected(String message) throws RemoteException {	
			}
	
			
			@Override
			public void connecting(String device) throws RemoteException {	
			}
			
			@Override
			public void connected(String device) throws RemoteException {	
			}
			
			
			/**
			 * Disconnect from the robot.  			
			 */
			@Override
			public void disconnect() throws RemoteException {				
				Log.d(TAG,"Disconnecting from the robot.");
				disconnectFromRobot();
			}
	
			
			/**
			 * Connect to the deviceAddress of robot.  
			 */
			@Override
			public void connect(String deviceAddress) throws RemoteException {
				Log.d(TAG,"Connecting to the robot[" + deviceAddress + "]");
				connectToRobot(_mAdapter.getRemoteDevice(deviceAddress));				
			}

			@Override
			public void drive(int speed, int direction) throws RemoteException {												
				Integer spd = speed;
				Integer dir = direction;
				
				
				Log.v(TAG,"Raw speed: " + spd + " and direction: " + dir);
				
				if(spd == 45 || spd == 0){
					Log.d(TAG,"Speed is at 45 ignore");
					return;	//Ignore 45 and 0
				}				
				
				else if(spd > 0){
					spd += 100;				
					if(spd > 150) spd = 150;					
				}else if(spd < 0){
					speed += 100;
					if(spd < 0) spd = 0;
				}
				
				Log.v(TAG,"Processed speed: " + spd + " and direction: " + dir);
				
				byte[] forward = new byte[9];
				
				forward[0] = (byte) ((109) & 0xff);
				for(int i=1; i <= 8; i++ ){
					forward[i] = (byte) (0 & 0xff) ;
				}
				
				
				
				if (spd > 100) {		//moving forward.
					if (dir < 0) {
						// turn left						
						Log.v(TAG, "Turning left");
						if(spd - Math.abs(dir) < 100){						
							forward[1] = spd.byteValue();							
							forward[2] = ((byte) (100 & 0xFF));			//Stopping one wheel							
						}else{								
							forward[1] = spd.byteValue();							
							forward[2] = ((byte) ((spd  - Math.abs(dir)) & 0xFF));							
						}
					} else {
						//Turn right
						Log.v(TAG,"Turning right");
						if(spd - Math.abs(dir) < 100){
							forward[1] = ((byte) (100 & 0xFF));			//Stopping one wheel							
							forward[2] = spd.byteValue();							
						}else{
							forward[1] = ((byte) ((spd - Math.abs(dir)) & 0xFF));							
							forward[2] = spd.byteValue();							
						}
					}
				}else{ //MOVING BACKWARDS run at a standard speed and direction.
					forward[1] = ((byte) (REVERSE_SPEED & 0xff));
					forward[2] = ((byte) (REVERSE_SPEED & 0xff));
				}
				
				
				write(forward);				
				
			}


			@Override
			public void stop() throws RemoteException {
				Log.d(TAG,"Stopping Robot");
				write(new byte[] {(byte)(108 & 0xff),0,0,0,0,0,0,0,0});				
			}

			// THIS IS THE ACTUAL GET_SENSORS METHOD THAT GETS CALLED
			@Override
			public int[] getSensors() throws RemoteException 
			{							
				int[] result = write( new byte[] { (byte)(83 & 0xff) }, 256 * 192, 1000 ); // 83 = get photo ~147kb
				return result;
			}			
		};
		
			
	}
	

	

	private void setSensors(byte[] sensors) {
		this.sensors = sensors;		
	}

	private byte[] getSensors(){
			return this.sensors;
	}
	
	
	/**
	 * Initialize all the variables used in this activity.
	 */
	@Override
	public void onCreate(){
		Log.i(TAG,"onCreate() Called");
		
    	this._mAdapter = BluetoothAdapter.getDefaultAdapter();
    	this.sensors = new byte[11];
    	this._mState = STATE_DISCONNECTED; 
    	this.robot = new Scribbler();
    	
		super.onCreate();
	}
	
	

	@Override
	public void onStart(Intent intent, int startid){
		Log.i(TAG,"onStart() Called");
		super.onStart(intent, startid);
	}

	
	@Override
	public void onDestroy(){
		Log.i(TAG, "onDestroy Called");
		super.onDestroy();
	}
	
	
	

	
	
	
	
	
	
	
	
	  private synchronized void setState(int state,String message) {
	    	_mState = state;
	    	
	    
	    	if(state == STATE_CONNECTING){
	    		int i = mCallbacks.beginBroadcast();
	    		 Log.d(TAG,"Connecting Callbacks: "+i);
	    		 while (i > 0) {
	    		     i--;
	    		     try {
	    		         mCallbacks.getBroadcastItem(i).connecting(message);
	    		     } catch (RemoteException e) {
	    		         // The RemoteCallbackList will take care of removing
	    		         // the dead object for us.
	    		     }
	    		 }
	    		 mCallbacks.finishBroadcast();
	    	}else if(state == STATE_DISCONNECTED){

	    		int i = mCallbacks.beginBroadcast();
	    		 Log.d(TAG,"Disconnected Callbacks: "+i);
	    		 while (i > 0) {
	    		     i--;
	    		     try {
	    		    	 
	    		         mCallbacks.getBroadcastItem(i).disconnected(message);
	    		     } catch (RemoteException e) {
	    		         // The RemoteCallbackList will take care of removing
	    		         // the dead object for us.
	    		     }
	    		 }
	    		 mCallbacks.finishBroadcast();
	    	}else if(state == STATE_CONNECTED){
	    		int i = mCallbacks.beginBroadcast();
	    		 Log.d(TAG,"Connected Callbacks: "+i);
	    		 while (i > 0) {
	    		     i--;
	    		     try {
	    		    	 
	    		         mCallbacks.getBroadcastItem(i).connected(message);
	    		     } catch (RemoteException e) {
	    		         // The RemoteCallbackList will take care of removing
	    		         // the dead object for us.
	    		     }
	    		 }
	    		 mCallbacks.finishBroadcast();
	    	}
	    }
	    
	    public synchronized int getState() {
	        return _mState;
	    }
	    
	    
	    public synchronized void disconnectFromRobot(){
	    	this.stop();
	    }
	    
	    /**
	     * Start and initialize the connection to the robot.
	     * 
	     */    
	    private synchronized void start() {
	    	Log.d(TAG, "start");

	        // Cancel any thread attempting to make a connection
	        if (mConnectThread != null) {mConnectThread.cancel(); mConnectThread = null;}

	        // Cancel any thread currently running a connection
	        if (mConnectedThread != null) {mConnectedThread.cancel(); mConnectedThread = null;}	        	      
	    }
	    
	    
	    
	    /**
	     * Start the ConnectThread to initiate a connection to a remote device.
	     * @param device  The BluetoothDevice to connect
	     */
	    public synchronized void connectToRobot(BluetoothDevice device) {
	    	Log.d(TAG, "connect to: " + device);

	        // Cancel any thread attempting to make a connection
	        if (_mState == STATE_CONNECTING) {
	            if (mConnectThread != null) {mConnectThread.cancel(); mConnectThread = null;}
	        }

	        // Cancel any thread currently running a connection
	        if (mConnectedThread != null) {mConnectedThread.cancel(); mConnectedThread = null;}

	        // Start the thread to connect with the given device
	        mConnectThread = new ConnectThread(device);
	        mConnectThread.start();
	        
	  
	        setState(STATE_CONNECTING,device.getName());	        	       
	    }
	    
	    
	    /**
	     * Start the ConnectedThread to begin managing a Bluetooth connection
	     * @param socket  The BluetoothSocket on which the connection was made
	     * @param device  The BluetoothDevice that has been connected
	     */
	    public synchronized void _connected(BluetoothSocket socket, BluetoothDevice device) {
	        Log.d(TAG, "connected");

	        
	        // Cancel the thread that completed the connection
	        if (mConnectThread != null) {mConnectThread.cancel(); mConnectThread = null;}

	        // Cancel any thread currently running a connection
	        if (mConnectedThread != null) {mConnectedThread.cancel(); mConnectedThread = null;}


	        // Start the thread to manage the connection and perform transmissions
	        try {
				mConnectedThread = new ConnectedThread(socket);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
	        mConnectedThread.start();

	        
	      
	        	
	        setState(STATE_CONNECTED,device.getName());
	       
	        // Send the name of the connected device back to the UI Activity
	        

	        
	    }


	    /**
	     * Stop all threads
	     */
	    public synchronized void stop() {
	       Log.d(TAG, "stop");
	        if (mConnectThread != null) {mConnectThread.cancel(); mConnectThread = null;}
	        if (mConnectedThread != null) {mConnectedThread.cancel(); mConnectedThread = null;}

	        setState(STATE_DISCONNECTED,"Disconnected");
	    }
	    
	    /**
	     * Write to the ConnectedThread in an unsynchronized manner
	     * @param out The bytes to write
	     * @see ConnectedThread#writeSET(byte[])
	     */
	    public synchronized void write(byte[] out) {
	        // Create temporary object
	        ConnectedThread r;
	        // Synchronize a copy of the ConnectedThread
//	        synchronized (this) {
	            if (_mState == STATE_CONNECTED){
//	            r = mConnectedThread;
//	        }
	        // Perform the write unsynchronized
	            	mConnectedThread.writeSET(out);
	            }
	           
	    
	    }

	    
	    
	    public synchronized int[] write(byte[] out, int bytes, int sleep)
	    {
	    	if(_mState != STATE_CONNECTED)
	    		return null;
	    	else
	    		return mConnectedThread.writeGet(out, bytes, sleep);
	    }
	    
	    public synchronized int[] write(byte[] out, int bytes){
	    	if(_mState != STATE_CONNECTED) return null;
	    	else{
	    		return mConnectedThread.writeGet(out,bytes,1);
	    	}
	    }
	    /**
	     * Indicate that the connection attempt failed and notify the UI Activity.
	     */
	    private void _connectionFailed() {
	        setState(STATE_DISCONNECTED,"Unable to connect device");

	       
	        
	    }

	    /**
	     * Indicate that the connection was lost and notify the UI Activity.
	     */
	    
	    
		private void _connectionLost() {
	        setState(STATE_DISCONNECTED,"Device connection was lost");


	        
	    }


	    
	    
	    
	     
	    
	    
	    
	    
	    
	    
	    

    /**
     * This thread runs while attempting to make an outgoing connection
     * with a device. It runs straight through; the connection either
     * succeeds or fails.
     */
    private class ConnectThread extends Thread {
        private final BluetoothSocket mmSocket;
        private final BluetoothDevice mmDevice;

        public ConnectThread(BluetoothDevice device) {
            mmDevice = device;
            BluetoothSocket tmp = null;

            // Get a BluetoothSocket for a connection with the
            // given BluetoothDevice
            try {
            	
                tmp = device.createRfcommSocketToServiceRecord(SPP_UUID);                
            } catch (IOException e) {
                Log.e(TAG, "create() failed", e);
            }
            mmSocket = tmp;
        }

        public void run() {
            Log.i(TAG, "Beginning mConnectThread");
            setName("ConnectThread");

            // Always cancel discovery because it will slow down a connection
            _mAdapter.cancelDiscovery();

            // Make a connection to the BluetoothSocket
            try {
                // This is a blocking call and will only return on a
                // successful connection or an exception
                mmSocket.connect();
            } catch (IOException e) {
               
                // Close the socket
                try {
                    mmSocket.close();
                } catch (IOException e2) {
                    Log.e(TAG, "unable to close() socket during connection failure", e2);
                }
                // Start the service over to restart listening mode
                _connectionFailed();
                ScribblerService.this.start();
                return;
            }

            // Reset the ConnectThread because we're done
            synchronized (ScribblerService.this) {
                mConnectThread = null;
            }

            // Start the connected thread
            _connected(mmSocket, mmDevice);
        }

        public void cancel() {
            try {
                mmSocket.close();
            } catch (IOException e) {
                Log.e(TAG, "close() of connect socket failed", e);
            }
        }
    }
		
    
    
    

    /**
     * This thread runs during a connection with a remote device.
     * It handles all incoming and outgoing transmissions.
     */
    private class ConnectedThread extends Thread {
        private final BluetoothSocket mmSocket;
        private final InputStream mmInStream;
        private final OutputStream mmOutStream;

        public ConnectedThread(BluetoothSocket socket) throws InterruptedException {
            Log.d(TAG, "create ConnectedThread");
            mmSocket = socket;
            InputStream tmpIn = null;
            OutputStream tmpOut = null;
            int size = 0;
            
            // Get the BluetoothSocket input and output streams
            try {
                tmpIn = socket.getInputStream();
                tmpOut = socket.getOutputStream();
                
                
            } catch (IOException e) {
                Log.e(TAG, "temp sockets not created", e);
            }

            mmInStream = tmpIn;
            mmOutStream = tmpOut;
            
        	try {
        		
        		Log.d(TAG,"Setting up robot for passthrough mode");
        		    		        		
				mmOutStream.write(134);						
				Thread.sleep(100);							//Hack seems to be needed.
				size = mmInStream.available();				
				for(int i=0; i<size;i++) mmInStream.read();	//Handle the response.        	
        	
				mmOutStream.write(143);			
				Thread.sleep(100);							//Hack seems to be needed.
				size = mmInStream.available();
				
				for(int i=0; i<size;i++) mmInStream.read();	//Handle the response.
				
				Log.d(TAG,"Robot in passthrough mode");

				
				
				
				
				
        	} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
        }

        public void run() {
            Log.i(TAG, "Beginning mConnectedThread");
            while(mConnectedThread != null){
            	//keep alive.
            }
            Log.i(TAG,"Exiting mConnectedThread");

        }

        /**
         * Write SET command to the output stream.
         * SET command returns a 9byte echo followed by 11 bytes of sensor data
         * @param buffer  The bytes array to write
         */
        public void writeSET(byte[] buffer) {
        byte[] sensors = new byte[11];

       	 StringBuffer output = new StringBuffer();
       	 try {
       		 mmOutStream.write(buffer);
       		 
       		 Thread.sleep(100);					//Seems to be needed, there is a delay an receiving packets.
       		 for(int i=0; i<9;i++){
       			 output.append(mmInStream.read());
       		 }
       		 Log.d(TAG, "Reading 9 byte echo packet from robot: " + output.toString());       		
       		 Log.d(TAG,"Reading 11 byte sensor packet from robot.  Bytes read: " + mmInStream.read(sensors));
       		 
       		 //Clean up any extra bytes hanging around in the mmInStream Buffer.       		 
       		 Log.d(TAG,"Removed "+ flushInStreamBuffer()+ " bytes from the buffer.");
       		
       	 } catch (IOException e) {
       		 Log.e(TAG, "Exception during write", e);
       	 } catch (InterruptedException e) {
       		 Log.e(TAG,"Exception while sleeping thread",e);			
		}
        }

        
        /**
         * Clean up the BluetoothSocket InStream buffer.
         * 
         * @return the number of bytes flushed.
         * @throws IOException
         */
        private int flushInStreamBuffer() throws IOException{
        	int size = mmInStream.available();
        	//Clean up any extra bytes hanging around in the mmInStream Buffer.       		 
       		for(int i=0; i<size;i++) mmInStream.read();
       		
       		//Returning the number of bytes cleaned from the buffer
       		return size;
        }
        
        

        /**
         * Write GET command to the output stream.
         * GET command returns a variable length packet of data the user expects in return 
         * @param buffer  The bytes array to write
         */ 
        public int[] writeGet(byte[] buffer,int size, int sleep)
        {
        	int available, bytesRead = 0;
        	int[] output = new int[size];
        	
        	try
        	{
        		mmOutStream.write(buffer);
        		Thread.sleep(sleep);
        		
        		//for(int i = 0; i < 9; i++) { mmInStream.read(); } // ignore 9-byte echo packet
        		
        		available = mmInStream.available();
        		
        		while(available > 0 && bytesRead <= size)
        		{
        			for(int i = 0; i < available; i++)
        			{
        				output[bytesRead] = mmInStream.read();
        				bytesRead++;
        			}
        			Thread.sleep(5);
        			available = mmInStream.available();
        		}
        	}
        	catch(IOException ioe)
        	{
        		return new int[] { -1 };
        	}
        	catch (InterruptedException ie) 
        	{
        		return new int[] { -1 };
        	}
        	
        	
        	return output;
        	/*
        	byte[] output = new byte[size];
        	int bytesRead = 0;
        	
        	try {
				mmOutStream.write(buffer);
				Thread.sleep(sleep);
				
				for(int i = 0; i < 9; i++) { mmInStream.read(); } // ignore 9-byte echo packet
				
				bytesRead = mmInStream.read(output, 0, size);
				
	        	Log.d(TAG,"writeGET read " + bytesRead + " bytes.");
        	} 
        	catch (IOException e) 
        	{
				Log.e(TAG,"Exception while writing GET packet to the robot.",e);				
			}     
        	catch (InterruptedException ie) 
        	{
        		Log.e(TAG,"Exception while writing GET packet to the robot.",ie);
        	}
        	
        	return output;*/
        }                
        
        /**
         * Cleanup socket connections and configure robot back into 
         * non passthrough mode.
         */
        public void cancel() {
            int size=0;
        	try {              		
        		Log.d(TAG,"Stopping robot and turning off motors");
        		//TODO: call robot.stop();
        		byte stop[] = {(byte)(108 & 0xff),0,0,0,0,0,0,0,0};
        		mmOutStream.write(stop);
        		size = mmInStream.available();
            	for(int i=0; i<size;i++) System.out.print(mmInStream.read());
            	
            	Log.d(TAG,"Turnning of Scribbler Passthrough mode");
                mmOutStream.write(144);
            	size = mmInStream.available();
            	for(int i=0; i<size;i++) System.out.print(mmInStream.read());            	
            	
            	mmOutStream.write(143);
            	size = mmInStream.available();
            	for(int i=0; i<size;i++) System.out.print(mmInStream.read());
                
            	Log.d(TAG,"Scribbler Passthrough mode is deactivated");
            	mmSocket.close();
            	
            } catch (IOException e) {
                Log.e(TAG,"close() of connect socket failed", e);
            }
        }
    }

}